#include "drv_pid.h"

#include "math.h"

/**
 * @brief 初始化一个PID实例
 * @param pid: PID实例
 * @param pidInit: PID初始化结构体
 * @return None
 */
void PID_Init(Pid_t *pid, PidInit_t *pidInit)
{
    pid->kp = pidInit->kp;
    pid->ki = pidInit->ki;
    pid->kd = pidInit->kd;
    pid->form = pidInit->form;
    pid->target = 0.0;
    pid->output = 0.0;
    pid->currError = 0.0;
    pid->lastError = 0.0;
    pid->prevError = 0.0;
    pid->sumError = 0.0;
    pid->integralLimit = pidInit->integralLimit;
    pid->windupFlag = DISABLE;
    pid->maxOutput = pidInit->maxOutput;
    pid->minOutput = pidInit->minOutput;
}

/**
 * @brief 设置PID的目标值
 * @param pid: PID实例
 * @param target: 目标值
 * @return None
 */
void PID_SetTarget(Pid_t *pid, const float target)
{
    pid->target = target;
}

/**
 * @brief 设置PID的参数
 * @param pid: PID实例
 * @param kp: kp值
 * @param ki: ki值
 * @param kd: kd值
 * @return None
 */
void PID_SetArgs(Pid_t *pid, const float kp, const float ki, const float kd)
{
    pid->kp = kp;
    pid->ki = ki;
    pid->kd = kd;
}

/**
 * @brief 获取PID的输出值
 * @param pid: PID实例
 * @return PID的输出值
 */
float PID_GetOutput(Pid_t *pid)
{
    return pid->output;
}

/**
 * @brief 获取PID的目标值
 * @param pid: PID实例
 * @return PID的目标值
 */
float PID_GetTarget(Pid_t *pid)
{
    return pid->target;
}

/**
 * @brief 重置PID实例
 * @param pid: PID实例
 * @return None
 */
void PID_Reset(Pid_t *pid)
{
    pid->target = 0.0;
    pid->output = 0.0;
    pid->currError = 0.0;
    pid->lastError = 0.0;
    pid->prevError = 0.0;
    pid->sumError = 0.0;
    pid->windupFlag = DISABLE;
}

/**
 * @brief 进行一次位置式PID计算
 * @param pid: PID实例
 * @param feedback: 反馈值
 * @return None
 */
void PID_PositionalCalc(Pid_t *pid, const float feedback)
{
    pid->currError = pid->target - feedback;
    if (!(pid->windupFlag == ENABLE && signbit(pid->currError) == signbit(pid->output)))
    {
        pid->sumError += pid->currError;
    }
    float pOutput = pid->kp * pid->currError;
    float iOutput = pid->ki * pid->sumError;
    float dOutput = pid->kd * (pid->currError - pid->lastError);
    if (iOutput > pid->integralLimit)
    {
        iOutput = pid->integralLimit;
        pid->windupFlag = ENABLE;
    }
    else if (iOutput < -pid->integralLimit)
    {
        iOutput = -pid->integralLimit;
        pid->windupFlag = ENABLE;
    }
    else
    {
        pid->windupFlag = DISABLE;
    }

    pid->output = pOutput + iOutput + dOutput;
    if (pid->output > pid->maxOutput)
    {
        pid->output = pid->maxOutput;
    }
    else if (pid->output < pid->minOutput)
    {
        pid->output = pid->minOutput;
    }
    pid->lastError = pid->currError;
}

/**
 * @brief 进行一次增量式PID计算
 * @param pid: PID实例
 * @param feedback: 反馈值
 * @return None
 */
void PID_IncrementalCalc(Pid_t *pid, const float feedback)
{
    pid->currError = pid->target - feedback;
    float pOutput = pid->kp * (pid->currError - pid->lastError);
    float iOutput = pid->ki * pid->currError;
    float dOutput = pid->kd * (pid->currError - 2 * pid->lastError + pid->prevError);

    pid->output += pOutput + iOutput + dOutput;
    if (pid->output > pid->maxOutput)
    {
        pid->output = pid->maxOutput;
    }
    else if (pid->output < pid->minOutput)
    {
        pid->output = pid->minOutput;
    }
    pid->prevError = pid->lastError;
    pid->lastError = pid->currError;
}

/**
 * @brief 对PID实例进行计算
 * @param pid: PID实例
 * @param feedback: 反馈值
 * @return None
 */
void PID_Calculate(Pid_t *pid, const float feedback)
{
    if (pid->form == PID_POSITIONAL_FORM)
    {
        PID_PositionalCalc(pid, feedback);
    }
    else if (pid->form == PID_INCREMENTAL_FORM)
    {
        PID_IncrementalCalc(pid, feedback);
    }
}
